import java.awt.Color;
import java.awt.Point;
import java.awt.geom.Point2D;
import java.awt.image.ColorModel;
import java.lang.instrument.Instrumentation;
import java.util.ArrayList;
import java.util.PriorityQueue;
import java.util.Random;

import ownclasses.TrajNode;

import excaptions.AandBZeroException;

import ij.IJ;
import ij.ImagePlus;
import ij.ImageStack;
import ij.gui.NewImage;
import ij.plugin.filter.PlugInFilter;
import ij.process.ColorProcessor;
import ij.process.FloatProcessor;
import ij.process.ImageProcessor;


public class A08_Brute_Force_Algorithm implements PlugInFilter{

	ImagePlus img1;
	double omegaDir = 0.1;
	double omegaVel = 0.5;
	double distanceThreshold = -60;
	Point[][] allPoints;
	Random rg = new Random();
	int[][] traj;
	double[] coh;
	ArrayList<TrajNode> fT1;
	private static volatile Instrumentation globalInstrumentation;
	
	@Override
	public int setup(String arg0, ImagePlus img) {
		img1 = img;
		return DOES_8G + STACK_REQUIRED + NO_CHANGES;
	}
	
	@Override
	public void run(ImageProcessor ignore) {
		try{
		if(img1 == null)
		{
			IJ.log("Kein Stack");
		}
		ImageStack stack1 = img1.getImageStack();
		IJ.log("Stacksize "+img1.getStackSize());
		//Find points in first Frame
		ImageProcessor f1 = stack1.getProcessor(1);
		ArrayList<Point> points = new ArrayList<Point>();
		for(int i = 0; i < f1.getWidth(); i++) {
			for(int j = 0; j < f1.getHeight(); j++) {
				int[] rgb = new int[3];
				rgb = f1.getPixel(i, j, rgb);
				if(rgb[0] < 50 && rgb[1] < 50 && rgb[2] < 50) {
					points.add(new Point(i,j));
				}
			}
		}
		
		//Find all points in the video
		allPoints = new Point[stack1.getSize()][points.size()];
		int counter = 0;
		for(int frame = 1; frame <= stack1.getSize(); frame++)
		{
			ImageProcessor ip = stack1.getProcessor(frame);
			for(int x = 0; x < ip.getWidth(); x++){
				for(int y = 0; y < ip.getHeight(); y++) {
					int[] rgb = new int[3];
					rgb = ip.getPixel(x, y, rgb);
					if(rgb[0] < 50 && rgb[1] < 50 && rgb[2] < 50) {
//						IJ.log("new Point X: "+x+" y: "+y);
						allPoints[frame-1][counter] = new Point(x,y);
						counter++;
					}
				}
			}

//			IJ.log("__________________________________________");
			counter = 0;
		}
		
		//Find closest Point in 2nd Frame
		int[] sndRow = new int[points.size()];
		for(int i = 0; i < points.size(); i++) {
			Point P1 = allPoints[0][i];
			double dist = Double.MAX_VALUE;
			int x = 0;
			for(int j = 0; j < points.size(); j++) {
				Point P2 = allPoints[1][j];
				double d = Math.sqrt(Math.pow(P1.x - P2.x, 2)+Math.pow(P1.y - P2.y, 2));
				if(d < dist) {
					dist = d;
					x = j;
				}
			}
			sndRow[i] = x;
		}
		//Check if 2nd row is valid
		IJ.log("Zweite Reihe berechnen: "+check2ndRow(sndRow));
		
		//Fill array with all possibilities
		traj = new int[(int) Math.pow(points.size(),img1.getImageStackSize()-1)][img1.getImageStackSize()];
		coh = new double[(int) Math.pow(points.size(),img1.getImageStackSize()-1)];
		
		int counter2 = 0;
		//Fill first 2 colums
		for(int i = 0; i < traj.length; i++) {
			if(i < (traj.length/5)-1) {
				counter2 = 0;
			} else if(i > (traj.length/5)-1 && i < (2*traj.length/5)-1) {
				counter2 = 1;
			} else if(i > (2*traj.length/5)-1 && i < (3*traj.length/5)-1) {
				counter2 = 2;
			} else if(i > (3*traj.length/5)-1 && i < (4*traj.length/5)-1) {
				counter2 = 3;
			} else if(i > (4*traj.length/5)-1 && i < 5*traj.length/5) {
				counter2 = 4;
			}
			for(int j = 0; j < 2; j++) {
				if(j == 0) {
					traj[i][j] = counter2;
				} else {
					traj[i][j] = sndRow[counter2];
				}
			}
		}
		
		//Fill other 8 collums
		int counter0 = 0;
		for(int i = 0; i < traj.length; i++) {
			
			int[] fill = calcRow(counter0);
			for(int j = 2; j < traj[0].length; j++) {
				traj[i][j] = fill[j-2];
			}
			counter0++;
		}
		
		
		//Calculate coherence
		double summe = 0.0;
		for(int i = 0; i < traj.length; i++) {
			summe = 0.0;
			for(int j = 1; j < traj[0].length-1; j++) {
				double distP1P2 = calcDist(traj[i][j-1], traj[i][j], j);
				double distP2P3 = calcDist(traj[i][j], traj[i][j+1], j);
				
				Point p1 = allPoints[j-1][traj[i][j-1]];
				Point p2 = allPoints[j][traj[i][j]];
				Point p3 = allPoints[j+1][traj[i][j+1]];
				double tanA;
				double tanB;
				
				if(p1.x > p2.x && p2.x > p3.x) {
					tanA = Math.atan2(p1.y-p2.y, p1.x-p2.x);
					tanB = Math.atan2(p2.y-p3.y, p2.x-p3.x);
					if(Math.abs(tanA-tanB) > Math.toRadians(85)) {
						summe += -50000;
					} else {
						summe -= 30*Math.abs(tanB-tanA);
					}
				} else {
					tanA = Math.atan2(p2.y-p1.y, p2.x-p1.x);
					tanB = Math.atan2(p3.y-p2.y, p3.x-p2.x);
					if(Math.abs(tanA-tanB) > Math.toRadians(85)) {
						summe += -50000;
					} else {
						summe -= 30*Math.abs(tanA-tanB);
					}
				}
				double d1 = omegaDir * calcDir(traj[i][j-1], traj[i][j], traj[i][j+1], j);
				double d2 = omegaVel * calcVel(traj[i][j-1], traj[i][j], traj[i][j+1], j);
//				IJ.log("tanA: "+tanA+" tanB: "+tanB);
				if(distP1P2 > 100 || distP2P3 > 100) {
					summe += -8000;
				}
				if(distP1P2-distP2P3 < distanceThreshold) {
					summe += -5000;
				}
				summe += d1 + d2;
			}
			coh[i] = summe;
		}
		
		//Achtung Achtung dieser Log kann mehr als 10 Minuten benötigen, gibt das gesamte Array aus
		String out = "";
		for(int i = (int) ((traj.length/5.0)-10); i < traj.length/5.0+20; i++) {
			for(int j = 0; j < traj[0].length; j++) {
				out = out + traj[i][j] + " | ";
			}
			out = out + "  "+coh[i]+"\n";
			IJ.log(out);
			out = "";
		}
		
		//Find the 3 best coherence for each startingpoint and chack that no point is used 2 times
		fT1 = new ArrayList<TrajNode>();
		
	//StartingPoint 1
		//Find best coh
		TrajNode bestNode = findBestCoherence(0, traj.length);
		fT1.add(bestNode);
		
		//Find 2nd best
		TrajNode best2Node = find2ndBestCoherence(0, traj.length);
		fT1.add(best2Node);
		
		//Find 2nd best
		TrajNode best3Node = find3rdBestCoherence(0, traj.length);
		fT1.add(best3Node);
		
		//Find 4th best
		TrajNode best4Node = find3rdBestCoherence(0, traj.length);
		fT1.add(best4Node);
		
		//Find 5th best
		TrajNode best5Node = find3rdBestCoherence(0, traj.length);
		fT1.add(best5Node);
	
		
		//Aus finalTraj Bild erstellen
		
		ImagePlus fp1 = NewImage.createRGBImage("best", img1.getWidth(), img1.getHeight(), 1, NewImage.FILL_WHITE);
		ColorProcessor cp1 = (ColorProcessor) fp1.getProcessor();
		
		cp1 = drawTrajectoryToImage(fT1, cp1);
		
		(new ImagePlus("Trajectories: best 1", cp1)).show();
		
		IJ.log("Fertig");
		} catch (AandBZeroException e) {
			IJ.error(e.toString());
		}
	}
	
	private double calcDist(int i, int j, int frame) {
		Point P1 = allPoints[frame-1][i];
		Point P2 = allPoints[frame][j];
		double dist = Math.sqrt(Math.pow(P1.x-P2.x, 2)+Math.pow(P1.y-P2.y, 2));
		return dist;
	}
	
	private boolean checkIfSavedAlready(int i) {
		for(TrajNode t : fT1)
		{
			if(t.row == i)
			{
				return true;
			}
		}
		return false;
	}

	private TrajNode findBestCoherence(int start, int end) {
		int best = -1;
		double bestnum = -Double.MAX_VALUE;
		for(int i = start; i < end; i++)
		{
			if(!checkIfPointsUsedTwice(fT1, traj[i]))
			{
				if(coh[i] > bestnum)
				{
					best = i;
					bestnum = coh [i];
				}
			}
		}
		return new TrajNode(traj[best], best);
	}
	
	private TrajNode find2ndBestCoherence(int start, int end) {
		int best2 = -1;
		double bestnum = -Double.MAX_VALUE;
		for(int i = start; i < end; i++)
		{
			if(coh[i] > bestnum && !checkIfPointsUsedTwice(fT1, traj[i]) && !checkIfSavedAlready(i))
			{
				best2 = i;
				bestnum = coh[i];
			}
		}
		return new TrajNode(traj[best2], best2);
	}
	
	private TrajNode find3rdBestCoherence(int start, int end) {
		int best3 = -1;
		double bestnum = -Double.MAX_VALUE;
		for(int i = start; i < end; i++)
		{
			if(coh[i] > bestnum && !checkIfPointsUsedTwice(fT1, traj[i]) && !checkIfSavedAlready(i))
			{
				best3 = i;
				bestnum = coh[i];
			}
		}
		return new TrajNode(traj[best3], best3);
	}
	
	private boolean checkIfPointsUsedTwice(ArrayList<TrajNode> tn, int[] is2) {
		if(tn.size() > 0)
		{
			for(TrajNode t : tn) 
			{
				int[] is = t.array;
				for(int i = 0; i < is.length; i++)
				{
					if(is[i] == is2[i])
					{
						return true;
					}
				}
			}
		}
		return false;
	}

	private ColorProcessor drawTrajectoryToImage(ArrayList<TrajNode> ft, ColorProcessor ip) {
		IJ.log("Length: "+ft.size());
		int j = 0;
		for(TrajNode t : ft) {
			IJ.log("coh "+coh[t.row]+" "+t.row);
			ip.setColor(getColor(j));
			Point p = allPoints[0][t.array[0]];
			ip.moveTo(p.x, p.y);
			for(int i = 1; i < t.array.length; i++) {
				p = allPoints[i][t.array[i]];
				ip.lineTo(p.x, p.y);
				TrajectoryUtils.drawSquare(p, ip, 2);
			}
			j++;
		}
		return ip;
	}
	
	Color getColor(int c) {
		switch(c) {
		case 0:
			return Color.black;
		case 1:
			return Color.blue;
		case 2: 
			return Color.red;
		case 3:
			return Color.orange;
		case 4:
			return Color.MAGENTA;
			default:
				return Color.white;
		}
	}

	private boolean check2ndRow(int[] sndRow) {
		boolean ret = true;
		for(int i = 0; i < sndRow.length; i++)
		{
			for(int j = 0; j < sndRow.length; j++)
			{
				if(i == j){}
				else if(sndRow[i] == sndRow[j]){
					return false;
				}
			}
		}
		return ret;
	}

	private int[] calcRow(int num) {
		int[] ret = new int[8];
		int tmp;
		for(int i = ret.length-1;i >=0; i--) {
			tmp = num % 5;
			num = num / 5;
			
			ret[i] = tmp;
		}
		return ret;
	}
	
	private double calcDir(int i1, int i2, int i3, int frame) throws AandBZeroException {
		double erg = 0.0;
		Point P1 = allPoints[frame-1][i1];
		Point P2 = allPoints[frame][i2];
		Point P3 = allPoints[frame+1][i3];
		
		Point P12 = new Point(P2.x - P1.x, P2.y - P1.y);
		Point P23 = new Point(P3.x - P2.x, P3.y - P2.y);
		
		double zahl = (Math.sqrt(Math.pow(P12.x, 2)+Math.pow(P12.y, 2)) * Math.sqrt(Math.pow(P23.x, 2)+Math.pow(P23.y, 2)));
		if(zahl == 0.0) {
			IJ.log("P1X"+P1.x+" P1y"+P1.y+" P2x"+P2.x+" P2y"+P2.y+" P3x"+P3.x+" P3y"+P3.y);
			IJ.log("Dir0 P12x:"+P12.x+" P12y:"+P12.y+" P23x:"+P23.x+" P23y:"+P23.y);
			throw new AandBZeroException();
		}
		
		erg = 1.0 - (P12.x * P23.x + P12.y * P23.y) / zahl;
		return erg;
	}
	
	private double calcVel(int i1, int i2, int i3, int frame) throws AandBZeroException {
		double erg = 0.0;
		Point P1 = allPoints[frame-1][i1];
		Point P2 = allPoints[frame][i2];
		Point P3 = allPoints[frame+1][i3];
		
		Point P12 = new Point(P2.x - P1.x, P2.y - P1.y);
		Point P23 = new Point(P3.x - P2.x, P3.y - P2.y);
		
		double nenn = (Math.sqrt(Math.pow(P12.x, 2)+Math.pow(P12.y, 2)) * Math.sqrt(Math.pow(P23.x, 2)+Math.pow(P23.y, 2)));
		double zahl = (Math.sqrt(Math.pow(P12.x, 2)+Math.pow(P12.y, 2)) + Math.sqrt(Math.pow(P23.x, 2)+Math.pow(P23.y, 2)));
		if(zahl == 0.0) {
			IJ.log("Vel0 P12x:"+P12.x+" P12y:"+P12.y+" P23x:"+P23.x+" P23y:"+P23.y);
			throw new AandBZeroException();
		}
		
		erg = 1.0 - 2.0 * Math.sqrt(nenn) / zahl;
		return erg;
	}
}
